import socket
import time
import _thread
import rospy
from geometry_msgs.msg import Twist

SPARKBASE_AXLE_LENGTH = 0.266
SPARKBASE_MAX_LIN_VEL_MM_S = 500
SPARKBASE_PULSES_TO_MM = 0.28885
message = ''
status = 0
bearpi_ip = '192.168.1.113'
addr = ("192.168.1.113",10006)

def drive(linear_speed,angular_speed):
    left_speed_mm_s = int((linear_speed - SPARKBASE_AXLE_LENGTH * angular_speed / 2) * 1e3)
    right_speed_mm_s = int((linear_speed + SPARKBASE_AXLE_LENGTH * angular_speed / 2) * 1e3)
    return driveDirect(left_speed_mm_s, right_speed_mm_s)

def driveDirect(left_speed,right_speed):
    left_speed_mm_s = max(left_speed, -SPARKBASE_MAX_LIN_VEL_MM_S)
    left_speed_mm_s = min(left_speed, SPARKBASE_MAX_LIN_VEL_MM_S)
    right_speed_mm_s = max(right_speed, -SPARKBASE_MAX_LIN_VEL_MM_S)
    right_speed_mm_s = min(right_speed, SPARKBASE_MAX_LIN_VEL_MM_S)
    left_speed_pwm_s = left_speed_mm_s / SPARKBASE_PULSES_TO_MM
    right_speed_pwm_s = right_speed_mm_s / SPARKBASE_PULSES_TO_MM
    return drivePWM(left_speed_pwm_s, right_speed_pwm_s)

def drivePWM(left_speed_pwm_s, right_speed_pwm_s):
    buffer = []
    buffer.append("53")  # 0
    buffer.append("4b")  # 1
    buffer.append("c")  # 2
    buffer.append("0")  # 3
    buffer.append("18")  # 4
    data1 = int(left_speed_pwm_s) & 0xff
    buffer.append(str(hex(data1))[2:4])  # 5
    data2 = int(left_speed_pwm_s) >> 8 & 0xff
    buffer.append(str(hex(data2))[2:4])  # 6
    data3 = int(right_speed_pwm_s) & 0xff
    buffer.append(str(hex(data3))[2:4])  # 7
    data4 = int(right_speed_pwm_s) >> 8 & 0xff
    buffer.append(str(hex(data4))[2:4])  # 8
    sum = checkSum(buffer) & 0xff
    buffer.append(str(hex(sum))[2:4])  # 9
    buffer.append("d")  # 10
    buffer.append("a")  # 11
    data = ''
    for i in buffer:
        if int(i, 16) < 16:
            data += '0'
            data += i
        else:
            data += i
    return data
def checkSum(buffer):
    sum = 0
    # print(len(buffer))
    for i in buffer:
        sum += int(i, 16)
    return sum
def main():
    
    NUC = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

    # ---------------------------------------------------------------#
    _thread.start_new_thread(NUC_client, (NUC,))
    drivePWM(901, 901)


def doMsg(msg):
    print(drive(msg.linear.x,msg.angular.z))
    # data = hex(int(drive(msg.linear.x,msg.angular.z), 16))
    data = drive(msg.linear.x,msg.angular.z).decode('hex')
    NUC.send(data)



if __name__ == '__main__':
    bearpi_ip = input("input bearpi ip(example \"ip\"):")
    rospy.init_node("tcp_base")
    NUC = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    NUC.bind(addr)
    NUC.connect((bearpi_ip, 10006))
    print("connected")
    sub = rospy.Subscriber("cmd_vel",Twist,doMsg,queue_size=10)
    rospy.spin()
